/*
 * Indiana University Community Grid Computing Lab Software License,Version 1.1
 *
 * Copyright (c) 2002 Community Grid Computing Lab, Indiana University. 
 * All rights reserved.		
 * 
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 *
 * 3. The end-user documentation included with the redistribution,
 *    if any, must include the following acknowledgment:
 *      "This product includes software developed by the Indiana University 
 *       Community Grid Computing Lab (http://www.communitygrids.iu.edu/)."
 *    Alternately, this acknowledgment may appear in the software itself,
 *    if and wherever such third-party acknowledgments normally appear.
 *
 * 4. The names "Indiana Univeristy","Indiana Univeristy Pervasive Techonology
 *    Labs" and  "Indiana Univeristy Community Grid Computing Lab" must not be
 *    used to endorse or promote products derived from this software without 
 *    prior written permission. For written permission, please contact 
 *    http://www.communitygrids.iu.edu/.
 *
 * 5. Products derived from this software may not use "Indiana
 *    University" name nor may "Indiana University" appear in their name,
 *    without prior written permission of the Indiana University.
 *
 *
 * THIS SOFTWARE IS PROVIDED ``AS IS'' AND ANY EXPRESSED OR IMPLIED WARRANTIES,
 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY 
 * AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL 
 * INDIANA UNIVERSITY OR ITS CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
 * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
package cgl.sensorgrid.common;

/**
 * <p>Title: </p>
 *
 * <p>Description: Converts ITRF Positions to Lat, Lon, Height values</p>
 *
 * <p>Copyright: Copyright (c) 2005</p>
 *
 * <p>Company: CGL@IU</p>
 *
 * @author Galip Aydin, gaydin@cs.indiana.edu
 * Original C function is provided by hondo@geodetics.com
 * @version 1.0
 */
public class ITRFtoLatLon {
    public ITRFtoLatLon() {
    }

    // Use these values arguments "a" and "f" which are semi-major axis and
    // flattening (for the ellipsoid model):
    // ECEF spheroid parameters
    //    double ECEF_SEMIMAJOR_AXIS = 6378137.0;
    //    double ECEF_FLATTENING = 0.00335281066474;

    public double[] GetLatLonHeightRadians(double m_x, double m_y, double m_z) {
        double ECEF_SEMIMAJOR_AXIS = 6378137.0;
        double ECEF_FLATTENING = 0.00335281066474;
        double a = ECEF_SEMIMAJOR_AXIS;
        double f = ECEF_FLATTENING;
        double lat=0.0;
        double lon=00;
        double hi=0.0;

        double rsq = 0, r = 0, e = 0, rho = 0, nlatr = 0;
        double cphi = 0, dr = 0, dz = 0, dht = 0, dnlatr = 0;
        double flatfn = 0, funsq = 0, sphi = 0, g1 = 0, g2 = 0;
        int i = 0;

        if (m_x == 0 && m_y == 0 && m_z == 0) {
          //  System.out.println("Here");
            lat = 0;
            lon = 0;
            hi = 0;
            return null;
        }

        flatfn = (2.0 - f) * f;
        funsq = (1.0 - f) * (1.0 - f);

        rsq = m_x * m_x + m_y * m_y;
        r = Math.sqrt(rsq);
        double fX = m_x;
//    if(!(fX)) {
        if (fX != 0.0) {
            fX += 0.0001;
        }
        e = Math.atan2(m_y, fX);
        if (e < 0.0) {
            e = e + 2 * Math.PI;
        }

        rho = Math.sqrt(m_z * m_z + rsq);
        sphi = m_z / rho;
        nlatr = Math.asin(sphi);
        hi = rho - a * (1.0 - f * sphi * sphi);

        for (i = 0; i < 10; i++) {
            sphi = Math.sin(nlatr);
            cphi = Math.cos(nlatr);
            g1 = a / Math.sqrt(1.0 - flatfn * sphi * sphi);
            g2 = g1 * funsq + hi;
            g1 = g1 + hi;
            dr = r - g1 * cphi;
            dz = m_z - g2 * sphi;
            dht = dr * cphi + dz * sphi;
            hi = hi + dht;
            dnlatr = (dz * cphi - dr * sphi) / (a + hi);
            nlatr = nlatr + dnlatr;
            // if((fabs(dnlatr)<=(1.0e-14)) && ((fabs(dht)/(a + hi))<=(1.0e-14))) {
            if ((Math.abs(dnlatr) <= (1.0e-14)) &&
                ((Math.abs(dht) / (a + hi)) <= (1.0e-14))) {
                break;
            }
        }
        lat = nlatr;
        lon = e;
        //System.out.println("lat, lon, hi = " + lat + "\t" + lon + "\t" + hi);
        double[] ret = new double[3];
        lat = rad2deg(lat);
        lon = rad2deg(lon);
        if (lon > 180.0)
            lon -= 360.0;

        ret[0]=lat;
        ret[1]=lon;
        ret[2]=hi;
        return ret;
    }

   //converts radians to decimal degrees
    private double rad2deg(double rad) {
        return (rad * 180.0 / Math.PI);
    }

     //converts decimal degrees to radians
    private double deg2rad(double deg) {
      return (deg * Math.PI / 180.0);
    }



     /*::  This routine calculates the distance between two points (given the     :*/
    /*::  latitude/longitude of those points).

    /*::  Passed to function:                                                    :*/
    /*::    lat1, lon1 = Latitude and Longitude of point 1 (in decimal degrees)  :*/
    /*::    lat2, lon2 = Latitude and Longitude of point 2 (in decimal degrees)  :*/
    /*::    unit = the unit you desire for results                               :*/
    /*::           where: 'M' is statute miles                                   :*/
    /*::                  'K' is kilometers (default)                            :*/
    /*::                  'N' is nautical miles                                  :*/

/*    private double distance(double lat1, double lon1, double lat2, double lon2, char unit) {
      double theta = lon1 - lon2;
      double dist = Math.sin(deg2rad(lat1)) * Math.sin(deg2rad(lat2)) + Math.cos(deg2rad(lat1)) * Math.cos(deg2rad(lat2)) * Math.cos(deg2rad(theta));
      dist = Math.acos(dist);
      dist = rad2deg(dist);
      dist = dist * 60 * 1.1515;
      if (unit == "K") {
        dist = dist * 1.609344;
      } else if (unit == "N") {
            dist = dist * 0.8684;
        }
      return (dist);
    }*/


}
/*
 Here's a C++ routine for translation of member elements m_x, m_y, m_z to
 LLH:

 Use these values arguments "a" and "f" which are semi-major axis and
 flattening (for the ellipsoid model):
// ECEF spheroid parameters
 #define ECEF_SEMIMAJOR_AXIS 6378137.0
 #define ECEF_FLATTENING     0.00335281066474

void GECEFPosition::GetLatLonHeightRadians(double &lat, double &lon, double
&hi, double a, double f) const
{
    double rsq=0, r=0, e=0, rho=0, nlatr=0;
    double cphi=0, dr=0, dz=0, dht=0, dnlatr=0;
    double flatfn=0, funsq=0, sphi=0, g1=0, g2=0;
    int i=0;

    if(m_x==0 && m_y==0 && m_z==0) {
        lat = 0;
        lon = 0;
        hi = 0;
        return;
    }

    flatfn = (2.0 - f)*f;
    funsq = (1.0 - f)*(1.0 - f);

    rsq = m_x*m_x + m_y*m_y;
    r = sqrt(rsq);
    double fX = m_x;
    if(!(fX)) {
        fX += 0.0001;
    }
    e = atan2(m_y,fX);
    if(e<0.0) {
        e = e + 2 * PI;
    }
    rho = sqrt(m_z*m_z + rsq);
    sphi = m_z/rho;
    nlatr = asin(sphi);
    hi = rho - a*(1.0 - f*sphi*sphi);

    for(i=0; i<10; i++) {
        sphi = sin(nlatr);
        cphi = cos(nlatr);
        g1 = a/sqrt(1.0 - flatfn*sphi*sphi);
        g2 = g1*funsq + hi;
        g1 = g1 + hi;
        dr = r - g1*cphi;
        dz = m_z - g2*sphi;
        dht = dr*cphi + dz*sphi;
        hi = hi + dht;
        dnlatr = (dz*cphi - dr*sphi)/(a + hi);
        nlatr = nlatr + dnlatr;
        if((fabs(dnlatr)<=(1.0e-14)) && ((fabs(dht)/(a + hi))<=(1.0e-14))) {
            break;
        }
    }
    lat = nlatr;
    lon = e;
}

*/
